# -*- coding: utf-8 -*-
import cv2
import time
import numpy as np
import sys
import threading
import socket
import pickle
from queue import Queue
from queue import Empty as QueueEmptyException
from json_read4 import get_location

drawing = False

import sys

# reload(sys)
# sys.setdefaultencoding('gb2312')
import matplotlib.pyplot as plt
import matplotlib


def hist_init():
    global axarr
    # 设置matplotlib正常显示中文和负号
    matplotlib.rcParams['font.sans-serif'] = ['SimHei']  # 用黑体显示中文
    matplotlib.rcParams['axes.unicode_minus'] = False  # 正常显示负号
    f, axarr = plt.subplots(3, sharex=True, sharey=True)
    f.suptitle('直方图')
    # Bring subplots close to each other.
    f.subplots_adjust(hspace=0)
    # Hide x labels and tick labels for all but bottom plot.
    for ax in axarr:
        ax.label_outer()


def draw_hist(data):
    """
        绘制直方图
        data:必选参数，绘图数据
        bins:直方图的长条形数目，可选项，默认为10
        normed:是否将得到的直方图向量归一化，可选项，默认为0，代表不归一化，显示频数。normed=1，表示归一化，显示频率。
        facecolor:长条形的颜色
        edgecolor:长条形边框的颜色
        alpha:透明度
    """
    for i in range(len(data)):
        axarr[i].clear()
        axarr[i].hist(data[i], bins=range(-100, -29), normed=1, facecolor="blue", edgecolor="black", alpha=0.7)
    # plt.show()
    return 0


def onMouse(event, x, y, flags, param):
    global ix, iy, drawing, mode
    ix, iy = x, y
    if event == cv2.EVENT_LBUTTONDOWN:
        drawing = True
    elif event == cv2.EVENT_MOUSEMOVE:
        pass
    elif event == cv2.EVENT_LBUTTONUP:
        drawing = False


# 载入图像
# im = cv2.imread('20180327212842.jpg')
# cv2.imshow("pic", im)
# mat = cv2.Mat(100, 300, cv2.CV_8UC1)
# cv2.waitKey(0)
offsetX = 50
offsetY = 50
width = 1055
height = 1123
circle_point = [(1055, 1123), (1055, 0), (255, 113 + 160 + 200)]


def get_line_point(point1, point2, x=None, y=None):
    if x is None:
        if point2[1] != point1[1]:
            return int((y - point1[1]) * (point2[0] - point1[0]) / (point2[1] - point1[1]) + point1[0] + 0.5)
        else:
            return None
    else:
        if point2[0] != point1[0]:
            return int((x - point1[0]) * (point2[1] - point1[1]) / (point2[0] - point1[0]) + point1[1] + 0.5)
        else:
            return None


def get_line_point2(point1, point2, x=None, y=None):
    # y=kx+b b=y-kx
    # x=(y-b)/k
    point1 = (point1[0] + offsetX, point1[1] + offsetY)
    point2 = (point2[0] + offsetX, point2[1] + offsetY)
    x1 = (point1[0] + point2[0]) / 2
    y1 = (point1[1] + point2[1]) / 2
    if point2[0] == point1[0]:  # 与y轴平行
        k = 0
    elif point2[1] == point1[1]:  # 与x轴平行
        k = None
    else:
        k = (point2[1] - point1[1]) / (point2[0] - point1[0])
        k = -1 / k
    b = y1 - k * x1
    if y is None:
        if k is None:
            return None
        else:
            return int(k * x + b + 0.5)
    else:
        if k is None:
            return point1[0]
        elif k == 0:
            return None
        else:
            return int((y - b) / k + 0.5)


def get_center_point(point1, point2):
    return (int((point1[0] + point2[0]) / 2), int((point1[1] + point2[1]) / 2))


def get_true_point(point):
    return (point[0] + offsetX, point[1] + offsetY)


skinCrCbHist = np.zeros((height + 100, width + 100, 3), dtype=np.uint8)
skinCrCbHist[:] = 0


# print(skinCrCbHist)
def rectangle(img, pt1, pt2, color, thickness=None, lineType=None, shift=None):
    pt1 = (pt1[0] + offsetY, pt1[1] + offsetY)
    pt2 = (pt2[0] + offsetY, pt2[1] + offsetY)
    cv2.rectangle(img, pt1, pt2, color, thickness)


def putText(img, text, org, fontFace, fontScale, color, thickness=None, lineType=None, bottomLeftOrigin=None):
    org = (org[0] + offsetY, org[1] + offsetY)
    cv2.putText(img, text, org, fontFace, fontScale, color)


def circle(img, center, radius, color, thickness=None, lineType=None, shift=None):
    center = (center[0] + offsetY, center[1] + offsetY)
    cv2.circle(img, center, radius, color, thickness)


def draw_desk(img):
    rectangle(img, (1055, 113 + 160), (255, 113), (128, 128, 128), thickness=2)
    rectangle(img, (1055, 113 + 160 + 200 + 160), (255 - 105, 113 + 160 + 200), (128, 128, 128), thickness=2)
    rectangle(img, (1055, 113 + 160 + 200 + 160 + 200 + 160), (255, 113 + 160 + 200 + 160 + 200),
              (128, 128, 128),
              thickness=2)


def draw_rssi_point(img, fps=0):
    putText(img, "fps:%d" % fps, (0, 30), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (0, 255, 255))
    cv2.rectangle(img, (offsetX, offsetY), (width + offsetX, height + offsetY),
                  (255, 255, 255))
    # circle_point[1] = (1055, 0)
    putText(img, "Point 2", (1055 - 100, 0 + 30), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (255, 255, 255))
    circle(img, circle_point[1], 10, (255, 255, 255), thickness=-1)

    # circle_point[2] = (255, 113 + 160 + 200)
    putText(img, "Point 3", (255 - 100, 113 + 160 + 200 + 30), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1,
            (255, 255, 255))
    circle(img, circle_point[2], 10, (255, 255, 255), thickness=-1)

    # circle_point[0] = (1055, 1123)
    putText(img, "Point 1", (1055 - 100, 1123 - 30), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (255, 255, 255))
    circle(img, circle_point[0], 10, (255, 255, 255), thickness=-1)


cv2.namedWindow(u'Rssi Test', cv2.WINDOW_AUTOSIZE)
cv2.setMouseCallback('Rssi Test', onMouse)
cv2.imshow(u"Rssi Test", skinCrCbHist)
cv2.waitKey(1)

back = skinCrCbHist.copy()

radius = [10] * 3
global area_num
area_num = -1
global rssi_num
rssi_num = [[], [], []]
global target_position
target_position = (0, 0)


def draw_info(back):
    if drawing:
        dis1 = np.linalg.norm(np.array((ix - offsetX, iy - offsetY)) - np.array(circle_point[0]))
        dis2 = np.linalg.norm(np.array((ix - offsetX, iy - offsetY)) - np.array(circle_point[1]))
        dis3 = np.linalg.norm(np.array((ix - offsetX, iy - offsetY)) - np.array(circle_point[2]))
        cv2.line(back, (ix, iy), (circle_point[0][0] + offsetX, circle_point[0][1] + offsetY), (0, 255, 255))
        cv2.line(back, (ix, iy), (circle_point[1][0] + offsetX, circle_point[1][1] + offsetY), (0, 255, 0))
        cv2.line(back, (ix, iy), (circle_point[2][0] + offsetX, circle_point[2][1] + offsetY), (0, 0, 255))
        cv2.putText(back, "%0.2fm" % (dis2 / 100), (ix, iy - 30),
                    cv2.FONT_HERSHEY_COMPLEX_SMALL, 1,
                    (255, 255, 255))
        cv2.putText(back, "%0.2fm" % (dis3 / 100), (ix, iy),
                    cv2.FONT_HERSHEY_COMPLEX_SMALL, 1,
                    (255, 255, 255))
        cv2.putText(back, "%0.2fm" % (dis1 / 100), (ix, iy + 30),
                    cv2.FONT_HERSHEY_COMPLEX_SMALL, 1,
                    (255, 255, 255))


def draw_radius(back):
    circle(back, circle_point[0], radius[0], (0, 255, 255), thickness=5)
    circle(back, circle_point[1], radius[1], (0, 255, 0), thickness=5)
    circle(back, circle_point[2], radius[2], (0, 0, 255), thickness=5)


def draw_target(back):
    circle(back, target_position, 10, (0, 0, 255), thickness=-1)


def loop():
    fps_count = 0
    t = time.time()
    fps = 0
    hist_init()
    subplots = draw_hist([[-60, -70, -80], [-60, -70, -80], [-60, -70, -80]])
    # subplots = draw_hist([[-50, -70, -80]])
    while True:
        back = skinCrCbHist.copy()
        fps_count += 1
        if time.time() - t > 1:
            fps = fps_count
            fps_count = 0
            t = time.time()

        # 画垂直平分线
        point1 = (get_line_point2(circle_point[1], circle_point[2], y=offsetY), offsetY)
        point2 = get_center_point(circle_point[0], circle_point[1])[1]  # 0和1中点的y坐标
        point2 = (get_line_point2(circle_point[1], circle_point[2], y=point2 + offsetY), point2 + offsetY)  # 中心点
        point11 = (get_line_point2(circle_point[0], circle_point[2], y=offsetY + height), offsetY + height)
        point111 = (width + offsetX, get_line_point2(circle_point[0], circle_point[1], x=width + offsetX))

        if area_num == 1:
            cv2.fillConvexPoly(back, np.array((point1, point2, point111, get_true_point(circle_point[1]))), (0, 100, 0))
        else:
            cv2.fillConvexPoly(back, np.array((point1, point2, point111, get_true_point(circle_point[1]))), (0, 20, 0))
        if area_num == 2:
            cv2.fillConvexPoly(back,
                               np.array((point11, point2, point1, get_true_point((0, 0)), get_true_point((0, height)))),
                               (0, 0, 100))
        else:
            cv2.fillConvexPoly(back,
                               np.array((point11, point2, point1, get_true_point((0, 0)), get_true_point((0, height)))),
                               (0, 0, 20))
        if area_num == 0:
            cv2.fillConvexPoly(back, np.array((point11, point2, point111, get_true_point(circle_point[0]))),
                               (0, 100, 100))
        else:
            cv2.fillConvexPoly(back, np.array((point11, point2, point111, get_true_point(circle_point[0]))),
                               (0, 20, 20))
        cv2.line(back, point1, point2, (0, 255, 0))
        cv2.line(back, point11, point2, (0, 0, 255))
        cv2.line(back, point111, point2, (0, 255, 255))

        draw_desk(back)
        draw_rssi_point(back, fps)
        draw_radius(back)
        draw_target(back)
        draw_info(back)
        cv2.imshow(u"Rssi Test", back)
        draw_hist(rssi_num)
        plt.pause(0.000001)
        if cv2.waitKey(1) == ord('e'):
            break


ui_queue = Queue(20)
is_thread_run = True


def loop2():
    global target_position
    while is_thread_run:
        while is_thread_run:
            try:
                HOST = '127.0.0.1'
                PORT = 8888
                s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
                s.connect((HOST, PORT))
                print("linked")
                break
            except BaseException as e:
                # runnable = Runnable(lambda: ax.set_title('no link'))
                # ui_queue.put(runnable)
                print("linked failed")
                pass
            time.sleep(1)
        route_data_list = [[], [], []]
        for i in range(3):
            route_data_list[i].append(circle_point[i][0])
            route_data_list[i].append(circle_point[i][1])
            route_data_list[i].append(10)
        while is_thread_run:
            try:
                input_str = s.recv(1024)
                input_str = pickle.loads(input_str)
                global rssi_num, area_num
                rssi_num = input_str[1]
                route_near_index = input_str[2]
                if abs(route_near_index[0][1] - route_near_index[1][1]) > 2:
                    area_num = route_near_index[0][0]
                else:
                    area_num = -1
                if route_near_index[0][1] < -79:
                    area_num = -1
                for i in range(3):
                    if route_near_index[i][1] > -70:
                        radius[route_near_index[i][0]] = 150
                    elif route_near_index[i][1] > -75:
                        radius[route_near_index[i][0]] = 500
                    elif route_near_index[i][1] > -80:
                        radius[route_near_index[i][0]] = 700
                    else:
                        radius[route_near_index[i][0]] = 900
                    route_data_list[i][2] = radius[i]
                target_position = get_location(route_data_list, type=int)
                # input_str = input_str[0]
                # if input_str[0] != -1:
                #     radius[0] = (input_str[0] + 1) * 100
                # if input_str[1] != -1:
                #     radius[1] = (input_str[1] + 1) * 100
                # if input_str[2] != -1:
                #     radius[2] = (input_str[2] + 1) * 100
            except Exception:
                print("linked loss")
                break
        s.close()


t = threading.Thread(target=loop2, name='LoopThread')
t.start()
loop()
cv2.destroyAllWindows()
plt.close()
sys.exit(0)
